#ifndef _TIME_BASED_ASTAR_H_
#define _TIME_BASED_ASTAR_H_

#include <dispatch_system/definitions.h>
#include <dispatch_system/custom_movepatterns.h>
#include <queue>
#include <dispatch_system/local_costmap.h>

namespace moying
{
namespace navigation
{
    struct costcomp
    {
        bool operator() (const NodePtr& lhs, const NodePtr& rhs) const
        {
            return lhs->f > rhs->f;
        }
    };

    //局部3d地图信息
    struct Local3DMap
    {
        int min_cell_x; //x维度最小坐标,路径规划地图分辨率下
        int min_cell_y; //y维度最小坐标
        int max_cell_x; //x维度最大坐标
        int max_cell_y; //y维度最大坐标
        int min_time;   //最小时刻
        int max_time;   //最大时刻
        VehicleState start; //路径规划的起点
        VehicleState goal;  //路径规划的终点
    };

    

    //路径起点和终点特殊处理的点信息
    struct PathEndNodeInfo{
        std::vector<Node> nodes;    //特殊处理的node数组,这些node要附加到path中,该数组中元素的parent无效,父子关系和元素排列顺序对应.
        NodePtr parent;             //nodes中第一个节点的父节点
    };

    class TimeBasedAStar
    {   
        public:
            TimeBasedAStar();
            virtual ~TimeBasedAStar();

            bool init(const MoyingMapPtr &moying_map);

            bool searchPath(const VehiclePtr &vehicle, const VehicleState &start, const VehicleState &goal, PlanningResult &result);
            
            const boost::dynamic_bitset<>& get_closed(bool local_map=false){ 
                if(closed_ptr_ == nullptr)
                    return boost::dynamic_bitset<>();
                else
                    return local_map ? closed_3d_ : closed_2d_;
            }
            
            Local3DMap get_local_map(){return local_map_;}
            LocalCostMapPtr local_cost_map(){return local_cost_map_;}

            const std::vector<Coordinate>& get_collide_pt_vector(){return collide_pt_vector_;}
            void set_search_cell_area(SearchCellArea area){search_cell_area_ = area;}

        private:
            bool findClosestTheta(const VehicleState &state, char &theta);
            bool astarSearch(std::vector<Node> &node_path,bool local_path_plan=false);
            int heuristic(const NodePtr &a, const NodePtr &b);
            void addValidNeighbors(const NodePtr &current, bool &find_dest);
            void addValidNeighbor(const NodePtr &current, const MovePattern& move_pattern, bool &find_dest);
            bool closeToGoal(const NodePtr &curr, const NodePtr &goal);
            
            void resetNodes();

            // /**
            //  * @brief 根据路径规划结果占用时间地图
            //  * @param   [IN] result           路径规划结果
            //  * @return  无
            //  */ 
            // void occupyTimeMap(PlanningResult &result);
            // void constructPlanFromGraphPath(const VehicleState &start, const VehicleState &goal, const std::vector<Node>& node_path, PlanningResult &result);

            /**
             * @brief 在2d地图中规划出路径,并解决时间冲突问题
             * @param   [IN] start      机器人起始状态
             * @param   [IN] goal       机器人目标状态 
             * @param   [OUT]result     路径规划结果 
             * @param   [OUT] no_path_2d    2D地图是否有路径
             * @param  [IN] recursion   是否递归
             * @return  操作结果
             */ 
            bool searchPath2D(const VehicleState &start, const VehicleState &goal, PlanningResult &result,bool& no_path_2d,bool no_recursion=true);

            /**
             * @brief 检测路径与时间地图是否有冲突
             * @param   [IN] node_path           路径
             * @param   [OUT] conflict_node      冲突的节点
             * @return  true:有冲突, false:无冲突
             */ 
            bool checkPathConflictWithTimemap(std::vector<Node>& node_path,NodePtr& conflict_node);

            /**
             * @brief 替换局部路径
             * @param   [IN OUT]    global_result       全局路径搜索结果
             * @param   [IN]        local_result        局部路径搜索结果
             * @param   [IN]        local_start_node    局部起始节点
             * @param   [IN]        local_goal_node     局部目标节点
             * @param   [IN]        local_map           局部地图
             * @return  操作结果
             */ 
            bool replaceLocalPath(PlanningResult &global_result,PlanningResult &local_result,NodePtr local_start_node,NodePtr local_goal_node,Local3DMap local_map);

            /**
             * @brief 建立局部3维地图
             * @param   [IN]    global_result   全局路径规划结果
             * @param   [IN]    conflict_node   时间冲突的节点
             * @param   [OUT]   local_map       局部3维地图
             * @param   [OUT]   start_time      局部路径的起始时间
             * @return  操作结果
             */ 
            bool prepareLocal3DMap(const PlanningResult& global_result,const NodePtr& conflict_node,Local3DMap& local_map,std::chrono::system_clock::time_point& start_time);

            /**
             * @brief 回退路径节点到达时刻
             * @param   [IN]    global_result       全局路径规划结果
             * @param   [IN]    retreat_node        开始回退时刻的节点
             * @param   [IN]    retreat_time        回退的时刻数
             * @param   [OUT]   retreat_first_node  是否回退到了第一个节点
             * @return  操作结果
             */ 
            bool retreatPathNodeArriveTime(PlanningResult& global_result,const Node& retreat_node,int retreat_time,bool& retreat_first_node);

            /**
             * @brief 把局部node转换成全局node
             * @param   [IN]    local_map       局部地图
             * @param   [IN]    local_node      局部节点
             * @param   [OUT]    global_node     全局节点
             * @return  操作结果
             */ 
            bool localNodeToGlobalNode(const Local3DMap& local_map,const Node& local_node,Node& global_node);

            /**
             * @brief 把搜索节点每一维的坐标转换成node数组中的索引
             * @param   x           x坐标
             * @param   y           y坐标
             * @param   theta       朝向
             * @param   time        时间
             * @param   move_state  运动状态
             * @return  索引
             */ 
            unsigned int toIndex(int x,int y,int theta,int time,int move_state);

            /**
             * @brief 获取移动邻近点的路径
             * @param   [IN]    cur_state   当前点坐标
             * @param   [IN]    next_state  下一点坐标
             * @param   [IN]    start_node  当前节点
             * @param   [IN]    next_node   下一节点
             * @param   [OUT]   node_vec    node路径
             * @param   [IN]    start_point true:当前点为起点,false:邻近点为终点
             * @return  
             */ 
            bool getMoveToNeighbourPath(const VehicleState& cur_state,const VehicleState& next_state,const NodePtr start_node,const NodePtr next_node,
                    std::vector<Node>& node_vec,bool start_point=true);

            /**
             * @brief 计算下一节点cost
             * @param   [IN]    cur_node    当前节点
             * @param   [OUT]   next_node   下一节点
             * @param   [IN]    pattern     运动模式
             * @return  
             */
            bool calculateNodeCost(const NodePtr cur_node,NodePtr next_node,const MovePattern& pattern);

            //处理起始点
            bool handleStartCell(std::vector<Node> &node_path);

            //处理目标点
            bool handleGoalCell(const NodePtr current,const NodePtr next_node);

            bool isNodeCanPass(const NodePtr current,const NodePtr neighbor);

            void displayPath(const PlanningResult& result);

        private:

            // std::vector<NodePtr>* nodes_ptr_ = nullptr;//指向当前使用的搜索空间
            // std::vector<NodePtr> nodes_2d_; //2d平面地图搜索空间
            // std::vector<NodePtr> nodes_3d_; //局部3维地图搜索空间
            NodePtr** nodes_ptr_ = nullptr;//指向当前使用的搜索空间
            NodePtr* nodes_2d_ = nullptr; //2d平面地图搜索空间
            NodePtr* nodes_3d_ = nullptr; //局部3维地图搜索空间
            boost::dynamic_bitset<>* closed_ptr_ = nullptr; //指向当前使用的闭节点集合
            boost::dynamic_bitset<> closed_2d_;//2d平面地图使用的闭节点集合
            boost::dynamic_bitset<> closed_3d_;//局部3维地图使用的闭节点集合
            MoyingMapPtr moying_map_;
            CostMapPtr costmap_;
            Local3DMap local_map_;  //局部3维地图
            LocalCostMapPtr local_cost_map_ = nullptr;            //局部代价地图
            std::vector<PlanningResult> plan_results_;   //存储到目标点的多条路径
            std::vector<FootprintCell> no_pass_nodes_; //临时不可通行点
            std::chrono::system_clock::time_point path_plan_start_time_;    //路径规划开始时间
            SearchCellArea search_cell_area_;

            VehiclePtr vehicle_;
            std::multimap<unsigned int, NodePtr> open_;
            NodePtr start_node_;
            NodePtr goal_node_;
            VehicleState start_state_;  //起始点
            VehicleState goal_state_;   //目标点
            MapDimension dimension_;

            std::multimap<unsigned int, PathEndNodeInfo> path_start_nodes_;     //路径首节点特殊处理的点,map的key为值的数组最后一个元素的f值
            std::multimap<unsigned int, PathEndNodeInfo> path_end_nodes_;       //路径尾节点特殊处理的点,map的key为值的数组最后一个元素的f值

            sch::ConfigInfo cfg_info_;  //配置参数
            int begin_wait_time_=0;       //开始在一个node等待的时刻
            bool local_path_plan_ = false;   //是否在局部路径规划

            int global_node_size_   = 0;    //全局地图的node数量
            int local_node_size_    = 0;    //局部地图的node数量

            bool drive_on_side_ = false;    //是否靠边行驶,优先规划出靠边的路径
            CustomMovePatternsPtr move_patterns_in_map_ = nullptr;//机器人在地图中的运动模式

            //test
            std::vector<Coordinate> collide_pt_vector_;
            int collide_times_=0;
            int calculate_millisec = 0;
            std::vector<int> times_;

    };
    typedef std::shared_ptr<TimeBasedAStar> TimeBasedAStarPtr;
}
}
#endif //_TIME_BASED_ASTAR_H_
